#include "Sensor.h"
#include "Delay.h"
#include "UART.h"
#include "ti_msp_dl_config.h"

int decide;
int error;
uint8_t Sensor_Data;

// uint8_t gw_gray_serial_read() {
//   uint8_t ret = 0;
//   uint8_t i;

//   for (i = 0; i < 8; ++i) {
//     /* 输出时钟下降沿 */
//     DL_GPIO_clearPins(SENSOR_PORT, SENSOR_SENSOR_SCL_PIN);
//     delay_us(5); // 有外部上拉源(4k ~ 10k电阻)可不加此行

//     // OLED_ShowNum(4, 4, DL_GPIO_readPins(SENSOR_PORT, SENSOR_SENSOR_SDA_PIN), 3);
//     ret |= (DL_GPIO_readPins(SENSOR_PORT, SENSOR_SENSOR_SDA_PIN) ? 1 : 0) << i;

//     /* 输出时钟上升沿,让传感器更新数据*/
//     DL_GPIO_setPins(SENSOR_PORT, SENSOR_SENSOR_SCL_PIN);

//     /* 延迟需要在5us左右 */
//     delay_us(5);
//   }

//   return ~ret;
// }

uint8_t gw_gray_serial_read() {
  uint8_t ret = 0;
  uint8_t i;

  ret |= (DL_GPIO_readPins(SENSOR_PORT, SENSOR_SENSOR_1_PIN) ? 1 : 0) << 3;
  ret |= (DL_GPIO_readPins(SENSOR_PORT, SENSOR_SENSOR_2_PIN) ? 1 : 0) << 2;
  ret |= (DL_GPIO_readPins(SENSOR_PORT, SENSOR_SENSOR_3_PIN) ? 1 : 0) << 1;
  ret |= (DL_GPIO_readPins(SENSOR_PORT, SENSOR_SENSOR_4_PIN) ? 1 : 0);

  return ret;
}

void read_sensor_values(void) {
  Sensor_Data = gw_gray_serial_read();

  if (Sensor_Data == 0xFF) {
    decide = 4; // 十字路口 1 1 1 1 1 1 1 1   直行
    error = 0;
    Sensor_PID.Kdl = 0;
  } else if (Sensor_Data == 0x7E) {
    decide = 5; // 十字路口 0 1 1 1 1 1 1 0   直行
    error = 0;
    Sensor_PID.Kdl = 0;
  } else if (Sensor_Data == 0x3C) {
    decide = 6; // 十字路口 0 0 1 1 1 1 0 0   直行
    error = 0;
    Sensor_PID.Kdl = 0;
  } else if (Sensor_Data == 0x18) {
    decide = 1; // 十字路口 0 0 0 1 1 0 0 0   直行
    error = 0;
    Sensor_PID.Kdl = 0;
  } else if (Sensor_Data == 0x0F) {
    decide = 2; // 90度路口 0 0 0 0 1 1 1 1    右转90度
  } else if (Sensor_Data == 0x1F) {
    decide = 2; // 90度路口 0 0 0 1 1 1 1 1    右转90度
  } else if (Sensor_Data == 0xF0) {
    decide = 3; // 90度路口 1 1 1 1 0 0 0 0    左转90度
  } else if (Sensor_Data == 0xF1) {
    decide = 3; // 向上锐角 1 1 1 1 1 0 0 0    向上锐角
  } else if (Sensor_Data == 0x01) {
    error = 4; //          0 0 0 0 0 0 0 1
    Sensor_PID.Kdl = 1;
  } else if (Sensor_Data == 0x02) {
    error = 3; //          0 0 0 0 0 0 1 0
    Sensor_PID.Kdl = 1;
  } else if (Sensor_Data == 0x04) {
    error = 2; //          0 0 0 0 0 1 0 0
    Sensor_PID.Kdl = 1;
  } else if (Sensor_Data == 0x08) {
    error = 1; //          0 0 0 0 1 0 0 0
    Sensor_PID.Kdl = 1.62;
  } else if (Sensor_Data == 0x10) {
    error = -1; //          0 0 0 1 0 0 0 0
    Sensor_PID.Kdl = 1.62;
  } else if (Sensor_Data == 0x20) {
    error = -2; //         0 0 1 0 0 0 0 0
    Sensor_PID.Kdl = 1;
  } else if (Sensor_Data == 0x40) {
    error = -3; //         0 1 0 0 0 0 0 0
    Sensor_PID.Kdl = 1;
  } else if (Sensor_Data == 0x80) {
    error = -4; //         1 0 0 0 0 0 0 0
    Sensor_PID.Kdl = 1;
  } else if (Sensor_Data == 0x00) {
    decide = 20; // 0 0 0 0 0 0 0 0
    error = 0;
  } else if (Sensor_Data == 0x81) {
    decide = 20; // 1 0 0 0 0 0 0 1
    error = 0;
  }
}
